/*
 * direction.c
 *
 *  Created on: May 23, 2021
 *      Author: kirk_
 */

#include "direction.h"
#include "velocity.h"

double GyrZ_Offset;

void Turn(float a)
{
	Direction.Target += a;
}

void TurnCW(uint32_t a)
{
	Turn((float)a);
}

void TurnCCW(uint32_t a)
{
	Turn(-(float)a);
}


void DirectionInit(void* argument)
{
	uint32_t Period;

	(void)Period;

	/* Get arg */
	if(argument != NULL)
		Period = *(uint32_t*)argument;
	else
		Period = Default_Fetch_Period;

	/* Init params */
	KD_FloatPID_Init(&Direction, DirectionDownlink);
	KD_FloatPID_SetIndex(&Direction, KD_FloatPID_IndexP, 0.00);
	KD_FloatPID_SetIndex(&Direction, KD_FloatPID_IndexI, 0.00);
	KD_FloatPID_SetIndex(&Direction, KD_FloatPID_IndexD, 0.00);
	KD_FloatPID_SetIndex(&Direction, KD_FloatPID_Target, 0.00);
	KD_FloatPID_SetIndex(&Direction, KD_FloatPID_Tolerance, 0.00);

	/* Test param */
	Direction.Denominator = -1000;
	Direction.indexP = 0.20;
	Direction.indexI = 0.00;
	Direction.indexD = 0.00;
	Direction.Tolerance = 0.00;
	Direction.Target = 0.00;

	return;
}

void DirectionDownlink(KD_FloatPID_t* pid)
{
	if(&Direction == pid)
	{
		VelocityIncTarget(0, pid->Increment);
		VelocityIncTarget(1, -pid->Increment);
		VelocityIncTarget(2, pid->Increment);
		VelocityIncTarget(3, -pid->Increment);
#if 0
		PositionL.Target -= pid->Increment;
		PositionR.Target += pid->Increment;
#endif
#if Mech_Wheel
		VelocityIncTarget(0, pid->Increment);
		VelocityIncTarget(1, -pid->Increment);
		VelocityIncTarget(2, pid->Increment);
		VelocityIncTarget(3, -pid->Increment);
#endif
	}

	return;
}

/* Thread */

volatile double Yaw = 0;

void StartDirectionThread(void *argument)
{
	uint32_t Period;

	/* Check Arg */
	if(argument != NULL)
		Period = *(uint32_t*)argument;
	else
		Period = Default_Direction_Period;

	/* Start timer */
	osTimerStart(DirectionTimerHandle, Period);

	/* Infinite loop */
	for(;;)
	{
		osThreadSuspend(DirectionThreadHandle);
		/* Fetch mpu6050 */
		KDL_MPU6050_Get_Gyroscope(&MPU6050);
		MPU6050.GyrZ -= GyrZ_Offset;
		Yaw += MPU6050.GyrZ * (float)Period;
#if 0
		PRINTF("[GyrZ] %ld;\r\n", (int32_t)(MPU6050.GyrZ * 1000));
		PRINTF("[Yaw] %ld;\r\n", (int32_t)(Yaw));
#endif
	}
}

float DirectionGet(void)
{
	return (float)Yaw/1000.00;
}

uint32_t Direction_Get(void)
{
	if(Yaw < 0)
		return (uint32_t)(fabs(Yaw)+100000000);
	else
		return (uint32_t)Yaw;
}

void DirectionOffset(double offset)
{
	GyrZ_Offset = offset;
}

void DirectionTimerCallback(void *argument)
{
	/* USER CODE BEGIN DirectionTimerCallback */
	osThreadResume(DirectionThreadHandle);
	/* USER CODE END DirectionTimerCallback */
}
